clc;
clear;
% 2021.10.10
% 本程序是HW1.m的改进版
% 加入多输入，方便同时获取不同输入点的激励

% 参数：1200kg、(200, 200, 150)kN/m、(0.02, 0.02, 0.04)N·s/m
mass_m = 1200 * eye(3);
stif_m = 1000*[400 -200 0;
    -200 350 -150;
    0 -150 150];
% mass_m\stif_m
cri_damp_m = [0.02, 0.02, 0.04];
% 自振频率
% inv(mass_m)*mass_m=eye(3)
% [eig_vec, eig_val] = eig(inv(mass_m)*stif_m);
[eig_vec, eig_val] = eig(mass_m\stif_m);

% 如何对应频率
omega_m = sort(sqrt(diag(eig_val)))';     % 5.6419   14.7706   22.3607
i=1;
j=3;

% 由于omega_m(j)^2-omega_m(i)^2部分，无法计算i=1，j=2情况？
alpha = 2*omega_m(i)*omega_m(j)/(omega_m(j)^2-omega_m(i)^2)*(omega_m(j)*cri_damp_m(i)-omega_m(i)*cri_damp_m(j));
beta = 2*omega_m(i)*omega_m(j)/(omega_m(j)^2-omega_m(i)^2)*(-cri_damp_m(i)/omega_m(j)+cri_damp_m(j)/omega_m(i));

damp_m = alpha * mass_m + beta * stif_m;    % 19.5959   19.5959   33.9411

% state matrix
A  = [zeros(3) eye(3);
    -mass_m\stif_m -mass_m\damp_m];     % -stif_m/mass_m -damp_m/mass_m
% input matrix
% 如何输入不同数据？
B = [zeros(3);eye(3)]/1200;
% output matrix [x1' x2' x3' x1" x2" x3"]
C = [1 0 0 0 0 0;0 1 0 0 0 0;0 0 1 0 0 0];
% throughtput matrix ?
D = [0 0 0;0 0 0;0 0 0];
% ss_structural
state_ss = ss(A, B, C, D);
trans_ss = tf(state_ss);
get(state_ss)

% conmput
t = 0:0.01:10;

% P如何作用在系统上
P1 = 100*sin(100*t)';
P2 = 100 * heaviside(t-2)';
P3 = 0*t';
% f1 f2 f3?

% a_problem
P = [P1 P2 P3];
% size(P0), size(P)

out_S = lsim(state_ss, P, t);
out_T = lsim(trans_ss, P, t);

% state space 与 tranform function 计算结果一致
% subplot(2, 2, 1);
% plot(t, P, t, out_S)
% subplot(2, 2, 2);
% plot(t, P, t, out_T)
% subplot(2, 2, 3);
% bode(state_ss)
% subplot(2, 2, 4);
% bode(trans_ss)

% subplot(1, 2, 1);
% plot(t, P, t, out_S)
% subplot(1, 2, 2);
bode(trans_ss)
